加速 度 セ ン 
活用 事例 


車輪 移動 型 倒立 振子 の 開発 に みる 


セン サ の 使い 方 


加速 度 セ ン サ と 肖 速 度 セ ン サ を 搭載 し た 倒立 振子 の 設計 事例 を 
紹介 する . 傾斜 角度 その も の を 直接 担 所 できない 角速度 セン サ 
だ け を 搭載 する と , セン サ や 周辺 回 路 の わずか な 誤差 の 蓄積 に 
より , 立ち 続け る こと が で き な い た め , 重力 検知 用 に 加速 度 セ 
ン サ を 併用 する . (編集 部 ) 


倒立 振子 は 文字 通り ,「 逆立ち し た 振り 子 」 で す . 普通 の 
振り 子 は 図 《 a) の よう に , 上 方 に 支持 点 が あり , そこ 
り が ぶら 下がっ て いて , ゆら ゆら 揺れ る と いう イメ ー ジ が 
あり ます . も う 少 し 厳密 に 表せ ば , 運動 する 物 條 剛体 ) の 
重心 が 支持 点 の 真下 に ある と き は 動か 平衡 点 入 り , い ず 
れ か の 方 向 に 傾い た 状態 で は , 平衡 点 に 戻 うう と する ガ 復 
元 力 ) が 働き ます . これ は 重力 に よっ て 引か れる た めで す . 
重り は 摩擦 な ど が な けれ ば いつ まで も 揺れ て いま す が , 普 
通 は 徐々 に 揺れ が 小さ く な っ で 減衰 ), 最後 に は 止ま り ま 
に 

その 振り 子 を 図 【 b) の よう に 逆立ち させ る と ( 重心 を 支 
持 点 の 上 方 に 持っ て 行く と ) どう な る で し ょ うか . 言う ま 


に 


Im 


(5 


( a) 普 通 の 振り 子 較 
図 1 振り 子 と 逆立ち し た 振り 子 


( b) 倒立 振子 較 


で も な く , 回 転 し て ぶら 下 が ろ うと し ます . 

で は , 完全 に 重心 を 支持 点 の 真 上 に 持っ て いく と どう な 
る で し ょ うか . 重り は 支持 点 の 真っ すぐ 上 に 乗っ て いる た 
め , 左右 に 傾く 力 は 働か ず , 動 こ うと は し ませ ん . ここ 
平衡 点 の 一 つ で す . た だ し , ほん の 少し で も 傾く と , ます 
ます 倒す 方 向 に 力 が 働く た め , あっ と いう 間 に 倒 れ て し ま 
いま す , 


⑯ ほう き が 倒れ な いた め の 制 御方 法 
この , 倒れ る の が 当たり 前 で ある 逆立ち し た 振り 子 を , 
倒れ な いよ うに 立た せ 続 ける こと は 可能 で す . 小学 校 で 掃 
除 の 時 間 に ほ うき を 手 の 上 に 立て て 遊ん だ 経験 の ある 方 も 
多い と 思い ます . これ が まさ に 倒立 振子 の 状態 で す . 倒れ 
る よう す を 見 な が ら , それ に 合わ せ て 手 を 動か す と , 立た 
せ 続 ける こと が で きま す . 
この 倒立 振子 は , 昔 か ら 制御 系 に 携わる エン ジニ ア の 関 
心 事 で し た . や り 方 に よっ て は で きる こと は 分 か る , で は 
「 具体 的 に どの よう に 」 と いう 問題 の 明確 さや , 一 般 に 入力 
が 一 つ で 制御 すべ き 値 が 4 種類 と いう 手ごろ な 複雑 さか ら , 
新しい 制御 理論 は まず , 倒立 振子 で 試す と いう 話 も あり ま 
ポ ファ ジー 制御 で 倒立 振子 な ど ). 


@ 倒立 振子 の 考え 方 は ロボ ッ ト の 足 に も 応用 され て いる 
その 一 方 で 近年 は , 移動 ロボ ッ ト の 足 と し て 注目 を 集め 
て いま す . 乗り 物 と し て 有名 な セグ ウェ イ ( Segway) も , 


KeyWord 加速 度 セ ン サ , 角速度 セン サ , 平衡 点 , ステ ッ ピ ング ・ モ ー タ , H8/3052, ADXRS150, ADXL202 
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人 間 を 振り 子 の 一 部 と する 倒立 振子 で す . また 日 立 製作 所 
の ロボ ッ ト の EMIEW も 2 車輪 で 動き ます . 何 も し な いと 
倒れ て し まう と いう こと は 危険 に 見 えま す が , 常に バラ ン 
ス を 取る こと で 思い が け な い 外部 か ら の 力 に 対応 で き , ま 
た , 機動 性 を 向上 させ る こと が で きま す . 

その ほか , 目 に 見 え に く い の で す が , 歩行 ロボ ッ ト も そ 
の 姿勢 制御 に 倒立 振子 的 な 考え を 入れ て いる 物 が あり ます . 
村田 製作 所 の ムラ タ セ イサ ク 君 2 号 も , 反動 車型 と 呼ば れ 
る 倒立 振子 に 分 類 さ れ ま す . この よう な 倒立 振子 に お いて 
は , 当然 で す が , その 傾き を いか に 知る か が 重要 で す 注 2. 
本 記事 で は , 図 1 に 示す 倒立 振子 の 解説 を 通し て , セン 
サ ・ デ バイ ス の 選定 , 信号 の 扱い , ソフ ト ウェ ア 実 装 に 
いて 述べ ます . 


1. 製作 する 倒立 振子 の 仕様 を 決め る 


今回 紹介 する 倒立 振子 は , 写真 1 に 示す よう な 機体 で , 
筆者 が か か わっ た 倒立 振子 の 中 で 6 台 目 に 当たり ます *3. 
この 倒立 振子 の 開発 動機 は , 卒業 研究 を する 学生 が 玉 乗 
り ロ ボッ ト を 作り た い 」 と 言い 出し た こと で す . いき な り 
玉 は 無理 だ か ら と 説得 し て まま 4「 パイ プ 乗 り ロ ボッ ト 」 に し 
まし た . その 上 で , 卒業 研究 の 範囲 で 手軽 に まとめ る た め 
に , 以下 の よう な コン セプト と し まし た . 


念 肛 動 は ステ ッ ピ ング ・ モ ー タ で 行う 

世間 一 般 に は , DC サー ボ ・ モ ー タ + 減速 機 を トル ク 指 
令 で 動か し ます が , さま ざま な 手間 が 増え ます . その た め , 
扱い や すい ステ ッ ピ ング ・ モ ー タ の 軸 に 車輪 を 取り 付け 
直接 , 駆動 し まし た . つい で に モー タ は 左右 独立 に し て 
旋回 な ど を 可能 に し まし た . 


@ 電池 や コン ピュ ー タ を 搭載 し た 自立 型 で ある こと 

倒立 振子 に 限ら ちず, 実験 室 で 装置 を 作る と き は , 電源 を 
外 に 置き , 制御 も パソ コン で , と いう 形 が 手 っ 取り 早い で 
し ょ う . た だ し , 今回 の 倒立 振子 の 場合 は , 電線 を 引っ 中 
る と 制御 に その 影響 が 出 や すい こと , パイ プ 乗 り と いう 移 
動 を 前 提 に し た 物 で ある こと か ら , 電源 も コン ピュ ー タ も 
搭載 し た 自立 型 に し まし た * ぜ 5. 

前 者 は , あく まず 手抜き 」 の た め に 決め た こと で す が , 
これ が 想定 外 に 極め て 高い 安定 性 を も た らし まし た . 後者 
に つい て も , いき な り デ モ し た りす る の に と て も 便利 な 出 


_ 事例 で マス タ す る 
加速 度 / 角 速度 セン サ o 使い 方 


写真 1 
加速 度 セ ン サ と 角速度 セン サ を 搭載 し た 
倒立 振子 


来 と な り ,「 バラ ンス ・ ロ ボッ ト 」 と し て , これ まで の 作品 
の 中 で 圧倒 的 な 稼働 率 を 誇っ て いま す . な お , 写真 1 は パイ 
プ 乗 り の た め の 機 能 は は ず し た , 単品 で 床 に 立つ 機体 で す . 


2. 倒れ な いよ うに 制御 する 方 法 


倒れ る 物 を 立て る た め に は , 引き 倒 そ うと する 重力 以上 
に , 逆 向 き に 作用 する 力 を 与え , それ を 調整 する 必要 が あ 
り ま す . 本 機 の 場合 は , 車輪 で 機体 の 下端 を 左右 に 加速 す 
る こと で , 上 が 倒れ て くる より も 速く , 下端 を 動か し て 立 
直し ます 6. 


念 考慮 すべ き 四 つ の 状態 が ある 
に 倒立 振子 の 制御 に お いて 考慮 する 四 つ の 状態 を 示 
し ます .( 0) は 目標 と する 状態 で す . 機体 は 水平 方 向 の 目 


注 2: ムラ タ セ イサ ク 君 も その 本 業 は 村田 製作 所 の セン サ の 広報 と 推測 され 
る 


注 3: 倒立 振子 マニ ア と いう わけ で は な い が , 気が付い た ら 研 究 用 や 学生 
験 用 な ど で 7 人 台 . 今年 さら に 2 人 台 追 加 の 予定 …. 
注 4: 今回 の 車輪 型 は 1 方 向 に し か 倒れ な い が , 玉 だ と 2 方 向 に 倒れ る ほか , 
ね じ れ も 生じ る . 開発 事例 も ほとん ど 見 られ な い . 

注 5: 持ち 運ん で 簡単 に デモ が で きる よう に , と いう 隠れ た 目的 も あっ た . 
メカ トロ ニク ス の 講義 や 高校 で の 出張 講義 な ど に お いて ,「 種 も 仕掛 
け も あり ます が 」 と 前 置き し て か ら 立た せる と , それ な り に 関心 を 引 


ける . 
注 6: 厳密 に は 全体 を 加速 する こと で 生じ る 慣性 力 に より , 重力 に よる 転倒 
の モー メン ト を 打ち 消し た り , さら に 立て 直し た りす る . 
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A 
(0 図 (as) 図 (b) 図 (c) 図 (gd)b 


2 倒立 振子 の 制御 に お いて 考慮 する 四 つ の 状態 


( a) は 単純 に 右 に 傾い て いる .( b) は 右 に 傾く 速度 が 付い て いる . ( c) は 目標 
位置 か ら 右 に ずれ て いる . ( d) は 右 に 動き つつ ある . 


標 位置 人 へ の と ころ で , 鉛直 に 立っ て いま す . この 状態 を 月 
す の は , ( a)^( d) の 四 つ の 状態 で す .( a) は 単純 に 右 に 傾 
いて いま す .( b) は 右 に 傾く 速度 が 付い て いま す .( c) は 目 
標 位置 か ら 右 に ずれ て いま す .( d) は 右 に 動き つつ あり ます . 
この 四 つ の 状態 を 補正 する に は , お の お の どう すれ ば よ 
いで し ょ うか . 答え は すべ て , 車輪 で 全体 を 「 右 に 加速 す 
る 」 で す .( a),( b) は 直感 的 に 理解 し や すい と 思い ます. 
問題 は , ( c),( d) で す . 右 に ずれ て いる , ずれ よう と し 
て いる の に , さら に 右 に 加速 と いう 矛盾 が あり ます . 
実例 を 考え て み ま す .( 0) の 状態 か ら , その まま 傾い て 
( a) に な っ た と し ます . 補正 する た め に は , 右 に 加速 し ま 
す . する と , 全体 的 に 右 に ずれ ます が , 角度 補正 は 瞬時 に 
で きる わけ で は な い の で , 右 に 傾き , 右 に ずれ た 状態 に な 
り ま す . その 状態 で , 右 に ずれ た か ら と , 左 に 加速 する と , 
ます ます 右 に 倒れ そう で す . よっ て , 左 に 加速 する わけ に 
は いき ませ ん . 0 ス を 無視 する と , 立 
て る こと だ け に 気 を 取ら れ , に よっ て は どん どん 加速 
の ほう き 立 て に お い 
て , 上 手 に 立て 続け られ る 人 の ほか に , 走れ る だ け 走 っ て 
いっ て し まう 人 が いま す が , まさ に この 状態 で す . 
消去 法 で は , 右 に 加速 する し か な さそ う で す . 右 に 大 き 
く 加速 する と , 下端 が 上 端 を 追い 抜き , 結果 的 に 左 に 倒れ 
る こと に な り 本 する と , この 左 に 倒れ た 振り 子 を 立て 


直す た め に ! に 加速 し な けれ ば な り ま せん . それ が 主 
作 と は 時 に な り , 元 の 場 
所 に 戻っ て くる わけ で す . これ は 直感 的 な 現象 の 説明 で す 


が , 制御 理論 的 に も 右 に ee 認 さ れ て いま 
す . この 動作 を 繰り 返す た め , 残念 な が ら ( 0) の 状態 で 静 
止 し て 立つ こと は で きま せん . 真っ すぐ 立っ て いる よう で 
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も わずか に 揺れ て いま す . 


人 @ 制御 する 四 つ の ゲイ ン 

以上 を も と に し た 制御 の 式 の 一 例 は , 傾斜 角度 9, 倒れ 
る 速度 角速度 ) o, 位置 x, 速度 > か ら , 出力 すべ き 加 束 
度 を 算出 する と , 


g ニ 9 二 友 の 大 メキ た De ( 1) 


と な り ま す . ここ で , 。, 。, た 。 包 は どの くら い 反 映 
させ る か を 決定 する 倍率 定数 ゲイ ン ) で す . この 四 つ の ゲ 
イン を 適切 に 調整 する こと で 立た せる こと が で きま す . 
通常 , 倒立 振子 の 制御 で は , 加速 度 で は な く , モー タ の 
出力 すべ き ト ル グ また は 水平 推力 ) を 求め ます . 条件 に も 
より ます が , 結果 に は さほど 違い が な いこ と 注 7? トル ク を 
調整 する に は 電流 を 調整 で きる 回 路 と 相応 に 特性 の 良い 
モー タ が 必要 な の に 対し て , 目標 加速 度 を 積分 すれ ば 目標 
速度 に な っ て 速度 の 制御 は し や すい こと な どか ら , この 式 
を 愛用 し て いま す . 特に 今回 は ステ ッ ピ ング ・ モ ー タ を 便 
う の で , ト ルク の 式 は 使え ませ ん 往 8. 


3. 加速 度 セ ン サ と 角速度 セン サ を 使う 理由 


人 @ 姿 替 の 検出 に 求め られ る 性 能 
地 1) に 必要 な , 位置 と 速度 は 自分 で モー タ を 回 し て い 
あの 9 で 。 自分 で 分 か る は ず で す 。、 ス テッ ピン グ ・ 邊 一 タ で 
な く と も , 通常 , モー タ に は 相応 の 回 転 セ ン サ を 付け て お 
きま す . そこ で , 倒立 振子 で は いか に 姿勢 角 を 測る か が 重 
要 に な り ま す . 位置 の 誤差 に 比べ , 姿勢 の 検出 誤差 は 鉛直 
が 狂う こと で あり , 制御 に と っ て 致命 的 で す . 倒立 振子 に 
求め られ る 姿勢 セン サ の 性 能 
e 十分 な 精度 を 持つ こと . 1 と ( 粗い 
と 動き が 粗く な る , 落ち 着 か な く な る ). 
e 十分 な 安定 性 を 持つ こと , つま り , 長 時 間 を た っ て も ゼロ 
は ゼロ の まま の こと ( ゼロ が 狂 え ば 倒れ る ). 
e 十分 な 応答 性 を 持つ こと ( 一 般 に , 制御 対象 が 小さ い ほ 
ど 高 い 周波 数 まで 必要 . 個人 的 目安 は 50Hz 程度 ). 
e 移動 の 加速 度 な ど , 姿勢 以外 の 事象 か ら 影 響 を 受け な い 


注 7: 高校 で 習う 運動 方 程 式 で は , /= ヵ z と , 力 と 加速 度 は 比例 し て いる . 
も ちろ ん そこ まで 単純 化 は で き な い . 

注 8: 逆 に 加速 度 の 式 に 慣れ て いた た め , ステ ッ ピ ング ・ モ ー タ で 作ろ うな 
ど と 考え た . 一 般 的 に は 直流 サー ボ ・ モ ー タ な ど を 使う . 


ル ! 電 
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図 3 

2 足 歩行 ロボ ッ ト に 加速 度 セ ン サ が 搭載 され た と き の 出 力 例 
機体 の 傾き を 加速 度 セ ン サ で 把握 する と き , セン サ 自 体 が 揺 す ら 
れる と 不 必要 な 動き の 加速 度 も 検出 し て し まう . 


この と 。 
な ど が 挙げ られ ます . 加え て 安けれ ば いう こと な し で す . 


人 @ 姿勢 検出 の 条件 を 満た す セ ン サ 

姿勢 セン サ と し て よく 知ら れ て いる 物 に , 角速度 セン サ 
( レー ト ・ ジ ャ イロ ) が あり ます 注 9 カメ ラ の 手ぶれ 補正 
や 注 10 自動 車 の 姿勢 検出 な ど に 使わ れ て いま すし , 歩行 
ロボ ッ ト に も 搭載 され て いま す . 物 に も より ます が , 比較 
的 多く の セン サ が 前 項 の 特性 を 満た し て いま す . 

ー つ だ け 注 意 し た い 点 が あり ます . それ は 検出 する 対象 
が 傾斜 角度 その も の で は な く , 角速度 で ある と いう 点 で す . 
角度 を 得る た め に は 時 間 で 積分 し な けれ ば な り ま せん . そ 
の た め , 例え ば 温度 の 変化 な ど で , 静止 状態 の セン サ の 出 
力 が わずか に 01"/& 1 時 間 で 1 回 転 する ほど の 速度 ) 程度 
ずれ た 場合 で も , 1005 た て ば 10" の ずれ と な っ て し まい ま 
す . 10' も ずれ れ ば , も は や 角度 の セン サ と し て 使い 物 に 
な り ま せん . も ちろ ん , セン サ 以 外 の 増幅 回 路 や AD コン 
バー タ の 誤差 で も 同様 で す . 


人 @ 衣 度 その も の を 検出 で きる セン サ も 併用 する 

そこ で , 通常 は 角度 その も の を 検出 で きる セン サ を 併用 
し ます . 以前 主流 だ っ た の は , 傾斜 計 と 呼ば れる , 振り 子 
こ 回 転 角 度 セ ン サ を 付け た よう な 物 で す . 振り 子 が 重力 方 


ーー 


水平 成分 較 


時 間 区 


着地 図 着地 図 着地 


胴体 の 上 下 動 や 凶 
着地 の 衝撃 図 


垂直 成分 較 


時 間 区 


重力 の 方 向 は ? 較 


向 を 向く た め , その 角度 を 測る こと で 傾斜 角 を 測れ ます . 
最近 は 性 能 が 向上 し て きた 加速 度 セ ン サ を 使う よう に な 
り ま し た . 地球 上 に は 至る 所 に 久 直 方 向 重力 加速 度 」 と 
いう 加速 度 が 働い て いま す . そこ で , これ を 加速 度 セ ン サ 
で 検出 すれ ば , 重力 の 方 向 が 分 か り ま す . し か し , これ ら 
の 重力 方 向 を 検出 する セン サ に も 弱点 が あり ます . セン サ 
自体 が 揺 す ら れる と , その 往復 の 加速 度 も 検出 し て し まう 
と いう 点 で す . 倒立 振子 も 左右 に 動き ます し , 歩行 ロボ ッ 
ト に 至っ て は 上 下 左右 に 激しく 揺れ ます . 

例え ば , 図 3 の よう に ロボ ッ ト が 一 定 の 角度 で 胴体 を 傾 
け て いる と き , 胴体 に 内 蔵 し た 加速 度 セ ン サ の 出力 は , 重 
力 だ け な ら グラ フ の 破線 の よう な 一 定 値 と な る は ず で す が , 
実際 に は ロボ ッ ト が 歩く と き の 左 右 の 重心 移動 や , 昌 
伴う 上 下 動 , 着地 衝撃 な ど で 実 線 の よう に 歩行 周期 に 
の 7 
ませ ん . その た め , 平均 値 に よっ て 長 時 間 に わ た る 絶対 的 
な 方 向 を 知る こと が で き て も , 短期 間 的 に は 役に立た な い 
千 果 が 出 て きま す . 


ml 


注 9: 角速度 セン サ と いう と コマ が 回 る 機械 的 な セン サ の イメ ー ジ が ある が 
単に 姿勢 の セン サ く らい の 意味 合い で , 普通 は 回 る 部 品 は 内 蔵 し て い 
な い . むし ろ , 圧電 素子 や MEMS 部 品 を 振動 させ る 品種 が 主流 . 

注 10: 手ぶれ は カメ ラ の 前 後 左 右 の 揺れ より も , 傾く 方 向 の 揺れ の ほう が 
像 の ぶれ は 大 きく なる. 
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目 目 目 / 目 


速度 セン サ 陣 
am | ンー 


速度 セン サ 隊 
加速 度 セ ン サ 陣 


( a) フ ィ ル タ を 2 個 用 いる 場合 較 


角速度 セン サ 陳 | 1 
K 
~ き +5 
+ 較 LPF 


加速 度 セ ン サ 隊 


( b) フ ィ ル タ を 1 個 に し た 場合 


図 4 加速 度 セ ン サ と 角速度 セン サ の 出力 を フィ ル タ で 合成 


そこ で , この 二 つ を 組み 合わ せる こと を 考え ます . 角 速 
度 セ ン サ は 短 時 間 の 性 能 に 優れ , 加速 度 セ ン サ は 長 時 間 の 
性 能 に 優れ ます . これ を 周 渡 数 で 考え る と , 角速度 セン サ 
は 周波 数 が 高い 方 で , 加速 度 セ ン サ は 周波 数 の 低い 方 で 優 
れる , と 言い 換え られ ます まほ 1. そこ で , 角速度 セン サ の 
信号 か ら ハ イ パ ス ・ フ ィ ル 人 HPF) で 欲し い 成 分 を , 加速 
度 計 か ら ロ ー パ ス ・ フィルタ ( LPF) で 欲し い 成 分 を 抜き 
取っ て か ら 混ぜ れ ば よい と 考え られ ます . 

図 4 a) は その 考え を 図示 し た 物 で す . 角速度 セン サ の 
信号 は あら か じ め 積 分 し て お いて , 角度 同士 で 混合 し ます . 
ここ で , 両 セ ン サ が 完 べ ぺき な セン サ だ と 仮定 する と , フィ 
ル タ を 通し て 混ぜ た 後に 特性 が 変化 し な いよ うに する た め 
に は , フィ ル タ の 特性 と し て , LPF 十 HPF = 1 と いう 関 
係 が 要求 され ます *12. 

する と , HPF = 1- LPF と 書き 直せ , 必要 な HPF を 
LPF で 実現 で きる こと に な り ま す . さら に , 2 系 統 で LPF 
を 共有 する よう に する と , 図 4 b) の よう に フィ ル タ を 1 個 
で 済ま せる こと が で きま す . アナ ログ 回 路 で 作る 場合 は 部 
品 が 半分 , デ ィ ジ タル ・ フ ィ ル タ で 実装 する 場合 も 演算 コ 
スト が ほぼ 半分 に な り ま す . これ を 減算 型 フ ィ ル ダ リ と 呼 
ん で いま す 往 13. 


mil 邊 目 | 


注 11: 一 般 的 な 電気 の 世界 の 発想 で は , どちら も か な り の 低 周波 数 . あく 
まで 相対 的 な 物 で , 境目 は 1Hz と か 01Hz など. 

注 12: 厳密 に は , 両者 の 伝達 関数 の 和 が 1 で ある . 

注 13: 大 学生 時 代 に か か わっ て か ら ず っ と 愛用 し て いる 手法 で , 修士 論文 

の テー マ の 半分 は これ . た だ し , 1 軸 単 位 で 測定 し て 混ぜ る 場合 に は 

使う が , 3 軸 で 大 きく 傾く と き は 適用 で き な い . 

注 14: 以前 , 東京 ・ 秋葉原 の 秋月 電子 で 販売 し て いた モー タ . 30 個 箱 買 い 
で 1 個 当たり 200 円 だ っ た . 

注 15: 重心 が 高い 方 が 倒立 振子 を 制御 し や すい こと は 定説 . 
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< 生 記 を こる 


写真 2 倒立 振子 の 足 は ステ ッ ピ ング ・ モ ー タ 2 個 で 構成 


今回 の 倒立 振子 も , 角速度 セン サ と 加速 度 セ ン サ , 減算 
型 フ ィ ル タ で 姿勢 検出 を 行っ て いま す . 


4. 倒立 振子 の 足 は ステ ッ ピ ング ・ モ ー タ 
2 個 で 構成 


メカ 部 分 は , 全体 の きょう 体 と な る 部 分 と 駆動 部 分 か ら 
な り ま す . 移動 の 要 で ある 駆動 部 分 を 写真 2 に 示し ます . 
ステ ッ ピ ング ・ モ ー タ 2 個 を 背中 合わ せ に 1 枚 の アル ミ 板 
に 固定 し , 全体 の 底板 に 当たる アル ミ 板 に 直角 に 固定 し て 
いま す . 普通 の ステ ッ ピ ング ・ モ ー タ は 軸 が 出 て いる 側が 
固定 面 な の で す が , た また まこ の モー タ 注 14 は 軸 の 反対 側 
が 固定 面 で , この 作り で 精度 良く 軸 が 一 致し まし た . モー 
タ に は , 直径 約 80mm の アル ミ 製 ホ イー ル に ゴム 製 リ ング 
を は め て タイ ヤ と し た 物 を 取り 付け て いま す . 

きよ う 体 部 分 は , アル ミ の アン グル を 4 本 用 意 し , 底板 
お よび 同じ サイ ズ の アル ミ 板 3 枚 と 組み 合わ せ , 回 路 と バッ 
テリ の 収納 が で きる よう に し まし 写真 1). この 部 分 は 
制御 に も 影響 し ます が , 経験 的 に パラ メー タ 出し を する 場 
合 に 厳密 さ は 必要 な く ,「 バッ テリ を そこ そこ 上 の 方 に 収 
納 す る よう に 」 往 1 と だ け 学 生 に 要望 し て , で きた の が この 
形 で す . 


5. 制御 ハー ドウ ェ ア の 開発 


電子 回 路 は きょう 体内 に 格納 し て いま す . 電源 回 路 を 除 
いて 写真 3 の よう に アク リル 板 に 固定 し た 上 で , まとめ て 


写真 3 制御 用 マイ コン 搭載 基板 や セン サ , セン サ 周 辺 回 路 搭載 基板 
の 外観 


取り 付け て いま す . 


@ 制御 用 マイ コン 

制御 に は ルネ サス テク ノロ ジ の マイ コン H8/3052」 を 
使っ て いま す . 必然 性 は 特に あり ませ ん が , 後述 の セン サ 
類 や モー タ 駆動 回 路 を 接続 し うる I/O 能力 が あり , な に よ 
り 「 使い 慣れ て いた 」 こ と が 選定 理由 で す . マイ コン は 秋月 
電子 通商 で 販売 され て いる マイ コン ・ ボ ー ド の 形 で 購入 し , 
自作 の ポー ト 分 配 基 板 一 種 の コネ クタ 変換 基板 ) に 載せ て 
使っ て いま す . 


@ セン サ の 周辺 回 路 
今回 は セン サ と し て , 角速度 セン が 角速度 を 検出 ) と 加 
速度 セン が 重力 加速 度 を 検出 ) を 使用 し まし た . 角速度 セ 
ン サ に は 米国 Analog Devices 社 の MEMS 角 速度 セン サ 
「 ADXRS150【 写真 4) を 採用 し , 加速 度 セ ン サ は 同社 の 
MEMS 加 速度 セン ガザ ADXL202」 を 採用 し まし た . 
角速度 セン サ と し て は , 村田 製作 所 , NEC ト ー キ ン な ど 


_ 事例 で マス タ す る 
加速 度 / 角 速度 セン サ 使い 方 


( a) 表面 
写真 4 角速度 セン ザ ADXRS150」 の 外観 


が 製造 し て いる 圧電 素子 系 の 品種 が 有名 で , 筆者 自身 も 歩 
行 ロ ボッ ト の 姿勢 セン サ な ど に よく 利用 し て いま し た . た 
また ま , 米国 Analog Devices 社 の MEMS 角速度 セン サ の 
プレ スリ リー ス を 知り , デー タ シ ー ト を 見 て その 性 能 に 稀 
き , 最近 は も っ ぱら 本 品 を 使っ て いま す . それ まで , 歩行 
ロボ ッ ト の 研究 な ど で カ ー ナ ビ 用 や 手ぶれ 補正 用 の 圧電 系 
の 角速度 セン サ を ( 単体 で は 不 十 分 だ っ た た め ) 組み 合わ せ 
て 使っ て いま し た が , それ ら の 応答 悼 上 限 の 周波 数 ) や 安 
定 怪 長 時 間 の ゼロ 点 出力 の 安定 性 ), 耐 加速 度 怪 衝撃 な 
ど に よる ノイ ズ ) の 実測 デー タ と 比較 し , ADXRS の カタ 
ログ ・ ス ペッ ク は 潜 編 し て いま し た . 実際 に 使っ て みて も , 
確か に 良好 で し た . 

本 機 に 搭載 し た セン サ は , 取り 引き 先 経 由 で 入手 し た サ 
ンプ ル 品 で , 国内 で は 普通 の 入手 経路 を まだ 見 つけ て いま 
せん . 現在 は 米国 の 通販 サイ ト Spark Fun Electrionics 注 16 
か ら , 角速度 セン サ お よび 加速 度 セ ン サ を 基板 に 実装 済み 
の 状態 17 で , 100 ド ル 前 後 で 購入 し て いま す . な お , 
ADXL202 は 秋月 電子 通商 で も 購入 で きま す . 

セン サ 周 り の 回 路 を 図 5 に 示し ます . 上 が 角速度 セン サ , 
下 が 加 速度 セン サ の 回 路 で す . ADXRS150 は 25V を 基準 
に , 角速度 に 比例 し た 電圧 が 出力 され る 仕様 で す . これ は 
フル スケ ー ル が 150"/s の セン サ で , ADXRS シ リー ズ で は 
感度 が 高い ほう で す が , 実際 に 倒立 振子 を 作っ て みた と こ 
ろ , その フル スケ ー ル ほど の 角速度 は 出 ま せん で し た . そ 
こ で 分 解 能 重 視 に する た が マイ コン の AD 変換 器 の 分 解 
能 が 10 ビ ッ ト し か な い ), 単 電源 OP ア ンプ で 25V を 基準 
に , 約 2 僅 に 増幅 し て いま す . 

ADXRS の 周り に コン デン サ が いく つか 付い て いま す が , 
これ は デー タ シ ー ト の まま と し まし が 内 部 で 使う 電源 用 
の コン デン サ な ど ). な お , 角速度 は H8/3052 の A-D 変 換 


注 16: http://www.sparkfun.com/ 
注 17: ADXRS は BGA ball grid array) パッ ケー ジ の た め , は ん だ 付け が 
困難 . 実装 済み が 便利 . 
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時 innllllllllll 


ヶ し oc《 5 十 ) 区 


H8 P7-0,1 
〒0.1 ん 7 
テ GND 
( 加速度 セン サ 図 
= Vc《 5 十 ) 

図 5 ェ H8 PB-0,1 
セン サ 周 り の 回 路 
ADXL202 は アナ ログ 量 の セン サ と し て は 珍し く , 検出 値 
を パル ス で 出力 する . ょ GND 
ポー ト の 2 本 に 入力 し て いま す . 二 つ の ポー ト の 変換 後 の @ ステ ッ ピ ング ・ モ ー タ 駆動 回 路 
値 を 加え る こと で , お まじ な い 程 度 に 分 解 能 の 向上 を 期待 ステ ッ ピ ング ・ モ ー タ は, N チ ャ ネル FET を 4 個 内 蔵 す 
し て いま す . 東芝 び MP4401」 で 駆動 し まし 図 6). コイ ル の 切り 

ADXL202 は アナ ログ 量 の セン サ と し て は 珍し く , 検出 替え 信号 励磁 信号 ) は, マイ コン 側 で 作っ て いま す . 今回 
値 を パル ス で 出力 し ます . ほぼ 一 定 周期 の パル ス を 出力 し , の モー タ は 定格 電圧 が 24V と 高い 一 方 , 電流 が 280mA と 
その ON 時 間 の 比率 デュ ー テ ィ 比 ) が, 加速 度 に 比例 し て 少な めで す 注 18 そこ で , 当初 は 72V の ニッ カド 電池 3 本 
変わ り ま す . 加速 度 ビ ロ で デュ ー テ ィ 比 が 約 50% で す . こ ( 22V 程度 ) で 動か し て いま し た 注 19. その後, 図 7 の 昇圧 回 
れ は この まま , マイ コン の パル ス ・ カ ウン タ に 接続 し ます . 路 往 20 を 開発 し , 追加 し まし た . この 回 路 は 米国 On Semi 
580 和 0699 RM の 窟 人 人 0 注 18: 市 販 ス テッ ピン グ ・ モ ー タ の 多く は , コイ ル の 定格 電圧 が 数 V と 低 

ヽ NE N 計測 が 可能 で す が , た い 代 わり に 電流 が 1A 以上 と 多く , モー タ 2 個 を 2 相 励 磯 する と すぐ 
ます っ の 6 の が 全 抽 た 2 失 RE ま 2 4 に トー タル で 5A を 若 え た り し て : 電源 設計 が 面倒 . 0 
また ま セ ン サ を は ん だ 付け し た 向き の 関係 で Y 軸 の 出力 だ 注 19: 2000mAh の 電池 で 2 時 間 以上 持つ . 

注 20: ロボ ッ ト に 取り 付け る だ け で 性 能 が 向上 する た め , 実験 室 で は テム ・ 

け を マイ コン に 送っ て いま す . レイ 回 路 笑 ) と 呼ば れ て いる . 


Bー 2 
A- 2 
BB 寺 2 
人 (C2 
Bー- 1 MP | 
人 っ lil 角 ら | 
B 十 1 EEQE4OO 区 3 
A 十 1 呈 震 呈 (ペン シン 7) 
10kx 8 ーーーーーーーー ト ーーーーー ビ ー レ ーーー ビ CN4 


図 6 

ステ ッ ピ ング ・ モ ー タ 駆動 回 路 
コイ ル の 切り 替え 信 富 励磁 信号 ) 
は , マイ コン 側 で 作っ て いる . 
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事例 で マス タ す る 
加速 度 / 角 速度 セン サ o 使い 方 


n 110g 昭 ロ 


図 7 

モー タ を 駆動 する た め 用 意 し 
た 昇圧 回 路 

無 負 荷 時 に は 約 40V が 得 ら れる . 


conductor 社 製 の MC34063」 を 用 いた 昇圧 回 路 で , 無 負荷 
時 に は 約 40V が 得 ら れ ま す . た だ し , モー タ 停止 状態 特 
定 の コイ ル に 直流 電流 が 流れ て いる と き ) か ら 低 速 回 転 時 
に は , 昇圧 回 路 の 入力 電流 制限 に よっ て ほとん ど 昇 圧 され 
ませ ん . 

一 方 , ステ ッ ピ ング ・ モ ー タ は その 性 質 上 , 回 転 を 速く 
する た め に コイ ル の 切り 替え 速度 を 高め る と , 電流 が 流れ 
に くく な り , 出力 トル 駆動 力 ) が 落ち る と いう 難点 が あ 
り ま す . この 回 路 は , その 電流 が 流れ に く い 状態 で は 可能 
な 範囲 で 昇圧 し て , 無理 や り 電流 を 流す よう に な り ま す . 
結果 的 に , 一 種 の 定 電力 駆動 の よう な 特性 が 得 られ , 低速 
域 で 無駄 に 電流 を 消費 する こと な く , 高速 域 の トル ク が 向 
上 し まし た . 

ステ ッ ピ ング ・ モ ー タ で は , トル ク 不 足 に よる 脱 調 が 致 
命 的 な 状況 を 起こ し ます が , この 倒立 振子 の 場合 は 即 , 転 
倒し ます . この 回 路 に よる トル ク 向上 で , 制御 中 に つつ い 
た り し て も 倒れ に くく な り ま し た 注 21. 


6. 制御 ソフ トウ ェ ア の 開発 


以降 , プロ グラ ム の 主要 部 を , ソー ス ・ コ ー ド を も と に 
解説 し ます . 制御 に か か わる すべ て の 部 分 に 共通 する 点 と 
し て , 固定 小数 点 演算 を 多用 し て いる こと が 挙げ られ ます . 
パワ ー の ある CPU で あれ ば , 浮動 小数 点 演算 回 路 が 付い 
て いた り , ソフ トウ ェ ア で エミ ュ レ ーション する 余力 も あ 
り ま す が , 16 ビ ッ ト CPU で 演算 頻度 を 確保 し た い 場 合 は 
採用 で きま せん . そこ で , 固定 小数 点 を 使い ます . 
固定 小数 点 は , 例え ば 16 ビ ッ ト の 変数 に 対し て , 上 位 8 
ビッ ト を 整数 部 , 下位 8 ビッ ト を 小数 部 と 人 
見 な し て , 演算 は 通常 の 整数 演算 を 行い, 最後 に けた 合 
せな ど を 行う 手段 で す . の 
数 に 28 倍 し た 値 で 格納 し て お く こ と に な り ま す . この と き 


所 3 0.22Q 5W 
WW ⑩ P+g 
/Cs MC34063 D Cs 本 GND 
330480V と ミ ら 
ビデ 補 
マツ 
ユ こ 且 の 
1000p エ 〒 記 


加減 算 よ は そのまま, 乗算 は 
(4x2 開 (px2 肢 - 4px2 た (4gx2 敵 <28 23 K 2 


と な り ま す . そこ で , 変数 同士 を 乗じ た 後 , 2 で 割る , つ 
まり , 8 ビッ ト 右 シ フト ( >>8) を 行え ば , 固定 小数 点 で の 
演算 が で きま す . 実際 に は , これ を より 強 備 ? ) し て , 各 
値 の 最大 の 有効 けた 数 を も と に , な る べく 精度 が 残る け 
た 落ち し な い ) よ うに , 小数 点 に 当たる 位置 を 決め て , 各 
式 で つじ つま を 合わ せ て いま す . また , 除算 は 演算 に 時 間 
が か か る た め , 避け て いま す . 


@ セン サ 信 号 処理 

リス ト 1 に セン サ の 信号 処理 2 
の 関数 は 約 8kHz の タイ マ 割 り 込み ま を の た びに 呼び 出さ れ 
ます . この 関数 で は , A-D コ ン バ ー タ 経由 で 角速度 セン サ 
回 路 の 出力 電圧 を ( AD コン バー タ を 2 系 統 使う こと で お 
まじ な い 程 度 に 分 解 能 向上 ), ITU _H8 の タイ マ ) 経 由 で 加 
速度 計 の 出力 信号 の ON 幅 を 取得 し て いま す . その 後 , 変 
数 gyrooEsc で 大 きく 二 つ に 分 か れ ま す . 前 半 は 通常 の 姿 
勢 角 度 演算 部 , 後半 は セン サ の オフ セッ ト 計測 部 で す . 

まず , 簡単 な オフ セッ ト 計測 部 を 見 て み ま す . オフ セッ 

ト 計測 と は , セン サ の オフ セッ ト , つま り , ゼロ 点 を 検出 
する こと で す . 仕様 上 , 角速度 セン サ は 角速度 が ゼロ で 
25V に , 加速 度 計 は デュ ー テ ィ 比 50% に な り ま す . し か 
し , 各種 誤差 こよ よる ずれ が あり , また 温度 変化 に よっ て 
徐々 に ずれ て きま ず 温度 ドリ フト ). その た め , あら か じ 
め ゼ ロ 点 を 決め て お く こ と は 難し く , 電源 投入 時 や ずれ て 


注 21: 実際 の と ころ は , 試し に 昇圧 回 路 を 作っ て みた ら , た また ま 良 い 特 
性 が 出 た の で , 解析 し た ら こ うだ っ た . その 後 量産 され , 実験 室 で 
ロボ ッ ト の 性 能 強 化 に 役立っ て いる . 

注 22: H8 で タイ マ 割 り 込み と いう と , も っ ぱら 通常 の タイ マ を 使う が , タ 
イマ を 他 の 目的 で 使い や すく する た め , 普段 か ら ウ ォ ッ チ ド ッ グ ・ 
タイ マ の イン ター バル ・ タ イマ ・ モー ド を 使っ て いる . 
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ゃ 


上 目 millll 目 


リス ト 1 セン サ の 信号 処理 部 分 の 関数 


YoO1d AitudeSensor (Vo1d) 

{ 
1nt gy,aC: 
1onq 上: 
// 生 値 読み 込み 
gy= (AD .DRA>>6) + (AD .DRB>>6) : // ジャ イロ 
ac=TTU3 . GRB : // 加速 度 セ ン サ 
1E (gyroofgc==0 ) // 通常 演算 モー ド 


dy エ CV= ( ( (1onq) 9y ) <<12 ) -dyroofg : 
yrOa+= (9Y エ OV+0x80 ) >>8 : 
aCCa=- ( ( ( (1onq) ac) <<12 ) -accofs ) *4 : 
七 =aCCa-9Y エ Ca : 
angF1 ヒ =angF1 モ 上 + 
( ((t-angE1t)+0x2000) >>14 ) : 
/* 0x2000-14 or Ox4000-15 */ 
and1e=dy エ Ooa+angF 1 : 


// オ フ セ ッ ト 除去 モー ド 


dyTOOfF 8+=qdY ツ : 

aCCOFF+=aC : 

Y エ OV=0 : dy エ Oa=0: aCCa=0: 
angF1 ヒ =0: ang1e=0: 

dy エ OOF 8O-- : 


きた と き に 直接 測定 し て し まう ほう が 現実 的 で す . 
具体 的 に は , ゼロ で ある 状態 本 機 の 場合 は 鉛直 静止 状 
で 値 を 計測 すれ ば よく , また , ノイ ズ の 影響 な ど を 避 
ける た め に ある 程度 の 回 数 の 測定 を 行っ て 平均 値 を 得 ます . 
この プロ グラ ム の 場合 は , メイ ン ・ ル ー チ ン 側 で , 角速度 
セン サ と 加速度 セ ン サ お の お の の オフ セッ ト 変数 gyroofg 
と accoFs を ゼロ に クリ ア し た 上 で , オフ セッ ト 計測 回 数 
カウ ンタ gyroofgc を 4096= 212 に セッ ト する こと で , オ 
フ セ ッ ト 計測 が 開始 し ます . 後半 部 を 4096 回 約 05s) 通 
る と , gyrooEsc が ゼロ に な り , gyorofs, acocofs に 
は お の お の の ゼロ 点 平均 値 の 212 倍 が 入り ます . 併せ て 制 
御 に 使う 角度 変数 群 を ゼロ に クリ ア し て いま す *23. 

前 半 は 角度 演算 部 分 で す . まず , 角速度 セン サ に 関す る 
演算 は , 角速度 セン サ の 検出 角速度 o。 を 積分 し て 角度 
9。 を 得 ます . 9。 は , 


上 


BR 


9。 = |ezw 時 ECHO イキ PF が PPFFKfTPFK ミ が FFP ( 3) 
で す が , サン プリ ング され た 値 で ある た め , 


%li- elly 


@。[= 9。-1 図 gl47 


98aResieids2s3 《 ④ 


と 置き 換え られ ます . 厳密 に 
合 は , サン プリ ング 周期 47 


SI 単位 系 で 計算 を 進め た い 場 
は 意味 を 持ち ます が , ここ で 
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は , 最終 的 に ゲイ ン 調 整 で 済ま せま ず ゲイ ン に 
時 0 
gyrov を , A-D 変換 か ら 得 た 生 の 値 か ら 先ほど の オフ セッ 
ト を 引く こと で 得 ま す . この と き , オフ セッ ト 変数 は 平均 
値 の 212 倍 に な っ て いる の で , 生 値 の ほう を 212 僅 , つま り , 
12 ビ ッ ト 左 シ フト ( <<12) し て いま す . いわ ば , 小数 点 の 
位置 が 12 ビッ ト 目 に あり ます . オフ セッ ト 計測 回 数 を 4096 
回 と 2 の 形 に し て いた の は , ここ で シフ ト ( 一 般 に 乗算 に 
比べ て 高速 ) で 済ま せる た めで す . 

炊 に 角速度 セン サ 角 度 gyroa に 加え 込ん で いま す . これ 
が 式 3③), 式 4) を 表し ます が , 加算 の 前 に 右 に 8 ビッ ト ・ 
シフ ト し て いま す . これ は 今後 の けた あふ れ を 考慮 し て , 
適当 に 有効 けた 数 を 削減 する た めで す . その 際 に , Ox80 を 
加え て いま す が , これ は 四捨五入 に 相当 し ます . 単に 右 シ 
フト を する と 切り 捨て に な り ま す が , 呈 人 NN 
当たる 数 の 半分 を 加え て いま す 注 史 . 積分 する 際 に 
の ちょ っ と の 部 分 が 大 きく 影響 し ます . 

同じ よう に 加速 度 セ ン サ に よる 角度 取得 を 行い ます . ま 
ず , オフ セッ ト を 引く と ころ は 同じ で す . 本 来 は , 鉛直 方 
向 お よび 水平 方 向 の 検出 加速 度 4。 4。 を も と に 


9 = an_ 衝 、 74.) Eee (5) 


で 角度 9、 を 求め る べき で す 往 25. し か し , 本 機 の 場合 は 傾 
斜 角 が 小さ いた め , タン ジェ ント の 近似 を 使っ て , 


月 =(4 74、) Se.8mte:318383ei88 計 GSSi8GTSieteie7eiSIS る as ( 6) 
さら に , 傾斜 が 小さ い 場 合 は , 4。 の 変化 は 少な いた 
め 注 36 適当 な 定数 ん , に よっ て , 

2 有 計 を 。 4、 83eCe 62880835B き 8387G888915X ま 7918C9 お が Re ( の 


と 書け ます . この 形 に する と , に セン サ や タイ マ の 変換 係 
数 まで 含め て し まう こと が で きま す . リス ト 1 の acca を 
求め る 式 で は , ① 冒 頭 の マイ ナス は 角速度 セン サ と 加速 度 


注 23: 単純 に メイ ン ・ ル ー チ ン で 4096 回 の ルー プ と せ ず に , わざ わざ 割り 
込み 処理 で 行う の は , あえ て 間隔 を 置き , 時 間 を か け て 計測 する こ 
と で , ノイ ズ な どの 影響 を 抽 え る も くろ み が あ る . 
注 24: 10 進 数 を 小数 点 以 下 を 削っ て 四捨五入 する 場合 は , 小数 点 以 下 第 1 
位 が 5 以上 で あれ ば 繰り 上 げ る , と いう 操作 だ が , 別 の 見 方 を する 
と , 05=1/2 を 加え て 切り 捨て る と いう 操作 で ある . 普通 の パソ コ 
ン な ど で 演 算 する 場合 も , qoub1e を int に キャ スト する と き , と 
きど き 05 を 加え る . 
注 25: 傾斜 角度 9 の と き , 4. 王 gcos 9 , 4, 三 gsin 6 g は 重力 加速 度 ) と 
な る た め , 4,/4. 三 tan 0 と な る . 
注 26: 4、 三 gcos 9 で , 9 が 小さ いう ち は ほ と ん ど cos 9 ニ 1 と 見 な せる た 
め . 


2 。 
症 目 旧 旧 時 ) 朋 ジ 提 


事例 で マス タ す る 
加速 度 / 角 速度 セン サ o 使い 方 


セン サ の 正負 が 不一致 だ っ た の で それ を 解消 , ⑳ *4」 は 
実験 的 に 求め た 角速度 セン サ に よる 角度 と 加速 度 セ ン サ に 
よる 検出 の 比 が た また ま 4: 1 で あっ た た め , 4 倍 し て 大 き 
さ を 一 致 さ せ た , と いう 形 に し て いま す . 

検出 の 比率 は , gyroa と acoa を 外部 に つない だ パソ コ 
ン か ら モニ タ し つつ , セン サ 回 路 を 数 度 傾け て みて , 値 の 
変化 か ら 求め まし た . 整数 比 こ な ら な か っ た 場合 は , [ *a) 
>>b」 の 形 , つま り ,( z/ ダ ) 倍 する こと に な り ま す . 今回 
の 用 途 に お いて 加速 度 セ ン サ は 鉛直 で ある か だ け を 見 て い 
れ ば 良い の で , この 程度 の 実装 で 十分 機能 し て いま す . 

その 後 は 前 述 の 角度 合成 の 演算 で , 最終 的 に 角度 angle 
( の) を 求め ます . 数 式 表現 は , 


9= の 9。+ Pr(9。 6。) USNBEEeRDesfeteaeeis ( 8) 


で す . この ロー パス ・ フ ィ ル タ の 実装 は , 1 次 の ディ ジ タ 
ル ・ フィルタ に し まし た 注 22. 式 は , 


ME0E3JL を ES27『4 Sa ば iS(828265 ae ( 9) 


た だ し , 入力 : 記 , 出力 : yk 7 : フィ ル タ の 特性 を 
決め る 微小 数 値 と する 
で す 注 8. ここ で , テテ 1/2 と 置き, 書き 換え る と , 
旭 = yk-]+(A[ 引 - 導 - 和 7 生生 と] ( 10) 


と な り , 加減 算 と シフ ト だ け の 式 に な り ま す . リス ト 1 で 
は angF1t が y[ 凡 に 当たり , ヵ テ 14 で , ここ で も 四捨五入 
相当 の 演算 を 行っ て いま す . 


@ モー タ 務 動 

ステ ッ ピ ング ・ モ ー タ は , ソフ トウ ェ ア で マイ クロ ス 
テッ プ 駆 動 を 行っ て いま す . 一 般 的 に は , ステ ッ ピ ング ・ 
モー タ で は 各 コ イル の 電流 の ON か OFF で 回 転 さ せま す . 
今回 使っ た よう な コイ ル が 4 本 ある タイ プ の モー タ の 場合 , 
順に 1 本 ずつ ON する こと で 一 定 の 角度 ずつ ステ ッ プ させ 
て 回 す 1 相 励磁 , 2 本 ずつ ON する 2 相 励 磁 , その 両者 を 組 
み 合 わせ 本 来 の ステ ッ プ の 半分 の 角度 ご と に 回 す 1-2 相 励 
磁 と いう 3 種類 が あり ます . 


今回 の モー タ の 場合 は , 標準 で 18'" ステ ッ プ な の で , 1- 


2 相 励 磁 で 09' に な り ま す が , 実際 に 動か し て みた と ころ , 
これ で は 粗い 動き に な り ま し た ま 29. そこ で , コイ ル に 流 
す 電 流 を ON/OFF す る だ け で は な く , 中 くら い の 電 流 な 
ども 使う こと で , 疑似 的 に 細か く 励磁 する マイ クロ ステ ッ 


リス ト 2 ステ ッ ピ ング ・ モ ー タ 駆動 部 


#deFine SUBPAT 6 
unsigned char STab]1e [32*SUBPAT] : 
Yolati1e unsigned 1nt SPCr, SBC] : 


Yo1d StepOut (Vo1d) 


unsiqned char Dp1] ,D エ : 
1n 上 g 上 1 了 , 8 上 1] 
SOseq= (SOseq+ 1 ) SUBPAT : 


SPC エ +=SSDpeedr : 
SPC1+=SSpeed] : 
8 キモ 1 ユエ = (SPC ェ >>11 ) &0xx1f : 
ヒュ 11= (SPC]1 >>11 ) g0x1F : 


8 ヒュ ユエ =8 ヒ ユエ + (SOsed<<5) : 
ヒュ 11 = 上 11+ (SOsed<<5) : 


pr=STab1e [8 モ ュ エ ] : 
p1=STab1e [sti1] : 
P1 .DR .BYTE=p エ | (p1<<4) , 


) 


プ と いう 方 法 を 使い まし た . 普通 は 専用 の 駆動 IC を 使い 
ます が , 今回 は ON の 時 間 比 率 を 調整 する PWM pulse 
width modulation, 加速 度 セ ン サ の 出力 と 同じ ) を 用 い , 
ソフ ト ウェ ア で 実装 し まし た . 具体 的 に は , ON 時 間 を 
( 0/6)( 3/6)( 5/⑥( 6/6) の 比率 と し まし 守 ON 時 間 と 電 
流 は 比例 し な いた め ). 

リス ト 2 は モー タ 駆動 の 関数 で , 8kHz 周期 で 呼ば れ ま 
す . 入力 は メイ ン ・ ルーチン で 指定 され る 両輪 の 回 転 速度 
SSpeedr, SSpeed1 で す . ここ で 変数 来 尾 の ェ , 1 は , 
モー タ の 右左 を 意味 し まず 以下 , 省略 ). 

まず , 指定 回 転 速度 SSpeed よ り , 盛 磁 位相 8PC を 求め 
ます . 励磁 位相 0~ 219- 1) は , 4 本 の コイ ル の 切り 替え 
1 セッ ト 分 に 当たり ます . 位相 の 増加 に 伴っ て , 励磁 する 
コイ ル が 順次 切り 替わり , オー バフ ロー 219) で 1 本 目 の コ 
イル に 戻り ます 注 30. ここ で は , 単純 に 8speed を 加え て 
いま す . 

例え ば , S8peed が 1 な ら ば , 65536 回 加算 し な いと 一 
巡 し ませ ん が , 88peed が 4 な ら , 16384 回 :「 1/4 の 時 
間 」:「 4 倍 の 速度 」 で 1 巡 し ます . これ は , 10 の よう な 
65536 を 割り 切れ な い 値 を 指定 し て も , 問題 な く 動作 し 
ます . 

次 に 上 位 5 ビッ ト を 切り 出し て , モー タ の 励磁 パタ ー ン 


注 27: 1 次 だ と 甘い の だ が , 整数 演算 で は 2 次 以上 は 困難 な た め . 

注 28: 極端 な 場合 と し て , テ 三 0 な ら 入 力 が 反映 され ず , 7 1 な ら 入 力 素 
通し . 

注 29: 車輪 が 直径 80mm 程 度 で ある た め , 1 ステ ッ プ で 06mm 程度 動く . 
注 30: モー タ の 仕様 で は 4 ステ ッ プ 分 に 当たり , 50 回 =200/4) あふ れる 
と モー タ 1 回 転 に な る . 
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を 得 て い ます . 5 ビッ ト ( 32 通 り ) で ある 理由 は , 1-2 相 励 昌 プリ ング 周期 を 乗じ る べき で す が , すべ て ゲ 
磁 の 8 通り を 基本 と し て , OR 4 段階 に 分 割 し イン 9 し まう こと で , 演算 を 削減 し て いま す . 
た た め , 8X 4 32 と な っ て いま す . こ さら に , この 
関数 が 呼ば れる た びに 0~ 5 で 変化 する 0 SOseg を 上 位 ⑯ パラ メー タ 通 信 設 定 
に 加え , その 時 点 で の ON/OFF を 定め た 数 表 STab1e [] 本 機 に 限ら ず マ イコ ン 制御 で ロボ ッ ト な ど を 作る 場合 , 
の イン デック ス sti に し て いま す . sti の 下位 を 固定 し た 内 部 の 状態 を モニ タ し た り , パラ メー タ 調整 を する 手段 は 
まま 上 位 を 順に 変え る と , 先ほど の ON 時 間 の 比率 に 従っ 不可 欠 で す . その 際 , 特に 制御 物 の 場合 , 実行 を 一 時 停止 
て ON/OFF が 決ま る よう に grTab1e [] を 作っ て お きま す . する な ど 本 来 の 制御 に 影響 を 及ぼ すこ と は で きず , バッ ク 
最後 に , モー タ が 接続 され て いる 1/O ポ ー ト か ら 値 を 出力 グラ ウン ド で 入出 力 で きる 必要 が あり ます . その 場合 , 科 
し ます . この 際 , 8kHz/6= 1.3kHz 周期 で ON に な り ま す . 易 的 に は ディ ジタル 入出 力 に LED や スイ ッ チ を 付け た り , 
その た め , モー タ か ら , 1.3kHz の 音 が ビー」 と 小さ く 聞 AD コン バー タ を 有用 いて パラ メー タ を 電圧 で 入れ た り ( 可 
こえ て きま す . 一 般 に は 耳障り と され て いま す が , ロボ ッ 変 抵抗 器 一 つ で 済む ), D-A コン バー タ か ら 変 数 を 出力 し 
ト 開発 者 に と っ て は ちょ うど 良い 動作 確認 音 で す . た り ( 連続 値 が オシ ロ で 確認 で きる と いう 意味 で は 非常 に 
便利 ) す る 方 法 が あり ます . た だ し , 分 解 能 を 確保 し た い 
@ 倒立 振子 制御 場合 や 値 の 種類 が 多い 場合 は 困難 で す . そこ で , シリ ア 
倒立 振子 制御 の 本 体 は 非常 に 簡単 で , リス ト 3 だ け で す ル ・ ポ ー ト 経由 で , パソ コン と 通信 する ライ ブラ リ を 作っ 
( 実際 に は , 各種 保護 や テス ト の コー ド が 数 倍 存 在 する ). て いま す . 詳細 は 触れ ませ ん が , 
th, thv, x,。 ゞ は それ ぞ れ 制御 に 必要 な 角度 , 角速度 , e シリ アル 通信 割り 込み だ け で 実装 し て ある た め , 本 来 の 
位置 , 速度 で す . 角度 , 角速度 は 姿勢 セン サ の 処理 値 を 適 処理 と 完全 に 分 離 限界 の 通信 速度 を 出し た と き に , CPU 
宜 シ フト し て 使っ て いま す . 速度 は 自分 自身 で モー タ を 回 時 間 の 20% 程 度 を 使用 ). 
し て いる の で その 値 を , 位置 は それ を 積算 し て 得 て い ます . e マイ コン 側 に は 16 ビ ッ ト ・ レ ジス タ と 32 ビ ッ ト の レジ 
倒立 振子 の 制御 式 に より , モー タ 加 速度 を 計算 し て い スタ を 各 16 本 持ち , パソ コン 側 か ら 専用 ソフ トウ ェ ア で 
ます . Reggi1et [] は , 後述 する パラ メー タ 変数 で , お の 通信 する こと で お の お の を 読み 書き で きる . 本 来 の 処理 
お の の 制御 ゲイ ン に し て いま す . 式 中 で >>14 し て いる よ の 側 か ら は , その レジ スタ に 代入 し た り , その 値 を 使う 
うに , これ も 固定 小数 値 で す . thofs, xofs は , 制御 開 の 強制 的 に パソ コン 側 に 値 を 送り つけ る こと も で き 
始 を 指示 し た 時 点 で の 角度 , 位置 で . ここ を 基準 th- る ). 
thofs を ゼロ に する よう に ) 制御 され ます . その た め , 真っ e 複数 の マイ コン を 数 珠 つ な ぎ に し た り , マイ コン 同士 の 
すぐ 立て た 状態 で 制御 を 開始 し ます . 求め た 加速 度 を 積算 通信 も 可能 
し て 目標 速度 refy に し て , これ を ステ ッ ピ ング ・ モ ー タ と いう 仕様 で す . 制御 部 の RegFi1er,[] は , この 32 ビ ッ 
へ の 指令 速度 に し て いま す . 左右 送 向 き に モー タ を 付け て ト ・ レ ジス タ で , パラ メー タ 調整 時 に は パソ コン 側 か ら 値 
いる の で , 指令 速度 が 正負 逆 で す . これ ら , 積算 の 式 も , を 適当 に 設定 し て は 制御 開始 指令 を 出し て いま し た 注 誰 . 
リス ト 3 メイ ン ・ ル ー チ ン 内 倒立 振子 制御 部 7. 秘伝 ? 倒立 掘 子 パ ラメ ー タ の 調整 法 
th=ang1e>>12 , 
thy=gyroy>>8』 さま ざま な 倒立 振子 を 作り, パラ メー タ を 調整 する うち 
0 に 身 に つけ た 方 法 を 参考 まで に 紹介 し ます . 
MReHRHGPRVIRH 
( (RegF11e [6] *(x-xoFg) ) >>14 ) + 
( (RegF11e [7] *Y) ) >>14 ) : 
TSEVmEGEVS7 注 31: 当初 は セン サ 初 期 化 , 制御 開始 な ども すべ て パソ コン 側 か ら 指示 し 
SSDGBGd エ そら エガ て いた . 現在 は 適切 な パラ メー タ も 最初 か ら マ イコ ン に 書き 込ん で 


SSDpeed エ =- エ efFV : 


あり , パソ コン 抜き で 動く . 
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ル ! 事例 で マス タ す る 
MPP I 紀 と \ー 仙人 議 加速 度 / 角 速度 セン サ 使い 方 


1. すべ て の ゲイ ン は ゼロ に し て お く . 

2. 手 で 倒立 振子 を 支え つつ , 角度 の ゲイ ン を 上 げ て いく . 
少し 倒す と 起き あがる 方 向 に 動い て くる よう に な る . 
3. 角度 の ゲイ ン を 上 げ る と , 動き 始め た と き に 行き すぎ 
て 往復 運動 発振 ) す る よう に な る . ここ で 角速度 の ゲ 

イン を 上 げ て いく と 収まる よう に な る . 

4. 角速度 の ゲイ ン で 調整 し つつ , 角度 の ゲイ ン を 上 げ て 
いく . する と , 指先 1 本 で 振り 子 の 上 端 を 支え る だ け で 
安定 し , 指 で 引っ 張る と スル スル と つい て くる よう に 
な る 注 32。 静止 し た 状態 か ら , そっ と 手 を 離す と 短 時 間 
は 立ち , その 後 , つつ 一 っ と 真っ すぐ 立っ た まま 走っ 
て いく ( 走ら せ す ぎる と 当然 倒れ る ). 

5. 次 に 位置 の ゲイ ン を 上げ て いく と , 指 1 本 で 引い た と き 
は 引い た 方 向 と 逆 向 き に 多少 倒れ て 抵抗 する よう に な 
り , 手 を 離し た 場合 は , つつ ー っ と いっ て 戻っ て くる 
よう に な る . た だ し , も と の 場所 を 大 きく 通り 過ぎ て 

発振 する . 

6. 速度 の ゲイ ン を 上 げ て いく と , 発散 し て いた 位置 が あ 
る 振幅 に 収束 する よう に な り , さら に 上 げ る と その 振 
上 205Yge 持続 的 に 立つ よう に な る . 

7. 完全 に 静止 直立 に は な ら な い が , 角度 ・ 角 速度 の ゲイ 
ン と 位置 ・ 速度 の ゲイ ン を 大 きく する と 揺れ の 振幅 は 
ある 程度 小さ く で き ゲイ ン を 上 げす ぎる と ノイ ズ な ど 
に 過敏 に な り 悪化 する ), それ ら の 比率 を 調整 する と 姿 
勢 と 移動 幅 の 振幅 を 調整 で きる . 
ちな み に 本 機 の 場合 , パラ メー タ 調整 を 始め て か ら 立つ 

まで , 5 分 か か り ま せん で し が 個人 的 最短 記録 ). 


注 32: お 散歩 モー ド と 呼ん で いる . これ を 応用 し て 2 輪 の 荷物 運搬 ロボ ッ ト 
を 試作 し た . 
注 33: 水 の 入 っ た コッ プ を 上 に 乗せ て も 怖く ない 程 度 . ス テッ ピン グ ・ モ ー 
タ で ある た め , 指定 し た 通り に 回 る 追従 の 良さ と , 間 に ギ ア な ど を 挟 
まな いた め , ガタ に よる 制御 の 悪化 が 無い こと が 勝因 と 見 て いる . 
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角速度 セン サ , 加速 度 セ ン サ の 応用 例 と し て , 倒立 振子 
の 実例 を 紹介 し まし た . 本 機 の 特徴 は , 角速度 セン サ , 加 
速度 セン サ を 併用 し て 姿勢 角度 の 検出 に 用 いて いる こと と , 
ステ ッ ピ ング ・ モ ー タ 駆動 で ある こと で す . セン サ の 併用 
こよ り , 電池 が 切れ る まで 2 時 間 で も 一 定 の 場所 で 立ち 続 
けら れる , 姿勢 検出 の 安定 性 を 確保 で きま し た . また , ス 
テッ ピン グ ・ モ ー タ の ダイ レク ト ・ ド ライ ブ に より , ほ と 
ん ど 動 か ず に 直立 に 近い 制御 を 実現 し まし た ま 8. ちな み 
に , お お も と の 目的 で あっ た パイ プ 乗 り は , この 倒立 振子 
に パイ プ か ら ずり 落ち な いよ うに する た め の ロ ー ラ を 取り 
付け て 行い ます . この と き , タイ ヤ と パイ プ の 摩擦 に より 
動力 が 伝達 され , パイ プ が 新た に 駆動 輪 と し て 振る 舞う よ 
うに な り ま す . パラ メー タ の 再 調整 は 必要 で す が , 安定 し 
て 制御 する こと が で き て いま す . さら に その 先 の 目標 で あ 
る 玉 乗 り は , 現在 計画 進行 中 で す が , ちょ うど 良い 玉 が 見 
つか ら な い の が 目下 の 課題 で す … 
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